#pragma once
namespace graph_virtual
{
namespace astar_search
{
   template<typename value_type>
   bool vertex_data_container_t<value_type>::has_key(graph_basic::vertex_id_t v_id)
   {
      typename data_t::iterator it1 = data_.find(v_id.graph_id_);
      if (it1 != data_.end())
      {
         typename std::map<graph_basic::vertex_in_graph_id_t, value_type>::iterator it2 = (*it1).second.find(v_id.vertex_in_graph_id_);
         if (it2 != (*it1).second.end())
            return true;
         else return false;
      }
      else 
         return false;
   }
   
   template<typename value_type>
   value_type vertex_data_container_t<value_type>::get_value(graph_basic::vertex_id_t v_id)
   {
      return data_[v_id.graph_id_][v_id.vertex_in_graph_id_];
   }
   
   template<typename value_type>
   void vertex_data_container_t<value_type>::set_value(graph_basic::vertex_id_t v_id, value_type val)
   {
      data_[v_id.graph_id_][v_id.vertex_in_graph_id_] = val;
   }
   template<typename value_type>
   value_type& vertex_data_container_t<value_type>::operator[](graph_basic::vertex_id_t v_id)
   {
      return data_[v_id.graph_id_][v_id.vertex_in_graph_id_];
   }
   template<typename value_type>
   typename vertex_data_container_t<value_type>::data_t::iterator begin()
   {
      return data_.begin();
   }
   template<typename value_type>
   typename vertex_data_container_t<value_type>::data_t::iterator end()
   {
      return data_.end();
   }
   
   template<astar_mode_e mode, astar_direction_e direction>
   astar_search_t<mode, direction>::astar_search_t
      (virtual_graph_view_ptr_t view, graph_basic::vertex_t& start_vertex, graph_basic::vertex_t& finish_vertex):
        view_(view), 
        start_vertex_id_(start_vertex.get_id()), 
        finish_vertex_id_(finish_vertex.get_id()),
        finish_lat_(finish_vertex.get_point().lat_),
        finish_lon_(finish_vertex.get_point().lon_),
        start_lat_(start_vertex.get_point().lat_),
        start_lon_(start_vertex.get_point().lon_)
    {
        init_state();
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::run(bool iterate, unsigned long long iteration_number)
    {
        if (direction == undirectional)
        {
            run1(iterate, iteration_number);
        }
        else if (direction == bidirectional)
        {
            run2(iterate, iteration_number);
        }
    }
    template<astar_mode_e mode, astar_direction_e direction>
    vertex_data_container_t<astar_vertex_data_t>& astar_search_t<mode, direction>::get_vertex_data()
    {
        return vertex_data1_;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    vertex_data_container_t<astar_vertex_data_t>& astar_search_t<mode, direction>::get_vertex_data2()
    {
        return vertex_data2_;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    output_list_t& astar_search_t<mode, direction>::get_output_list()
    {
        return output_list_;    
    }
    template<astar_mode_e mode, astar_direction_e direction>
    output_list_t& astar_search_t<mode, direction>::get_output_list2()
    {
        return output_list2_;    
    }    
    template<astar_mode_e mode, astar_direction_e direction>
    vertex_debug_t astar_search_t<mode, direction>::get_debug_vertex_by_id(graph_basic::vertex_id_t id, bool first_dim)
    {
       /* bool second_direction = is_in_opened_list2(id) || is_in_closed_list2(id);
        vertex_debug_t result;
        graph_basic::vertex_t& v = view_->get_vertex_by_id(id);
        if (second_direction)
        {
            result.distance_ = get_distance2(id);
            result.heuristic_ = graph_basic::math::my_astar_heuristic(v, start_lat_, start_lon_);
        }
        else 
        {
            result.distance_ = get_distance(id);
            result.heuristic_ = graph_basic::math::my_astar_heuristic(v, finish_lat_, finish_lon_ );
        }
        
        result.lat_ = v.latitude_;
        result.lon_ = v.longtitude_;
        result.id_ = v.id_;
        return result;*/
        vertex_debug_t result;
        graph_basic::vertex_t& v = view_->get_vertex_by_id(id);
        if (!first_dim)
        {
            result.distance_ = get_distance2(id);
            result.heuristic_ = graph_basic::math::my_astar_heuristic(v, start_lat_, start_lon_);
        }
        else 
        {
            result.distance_ = get_distance(id);
            result.heuristic_ = graph_basic::math::my_astar_heuristic(v, finish_lat_, finish_lon_ );
        }
        
        result.lat_ = v.get_point().lat_;
        result.lon_ = v.get_point().lon_;
        result.id_ = v.get_id();
        return result;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    graph_basic::vertex_t& astar_search_t<mode, direction>::get_vertex_by_id(graph_basic::vertex_id_t id)
    {
        return view_->get_graph().get_vertex_by_id(id);
    }
    template<astar_mode_e mode, astar_direction_e direction>
    graph_basic::lat_lon_t astar_search_t<mode, direction>::get_potential(graph_basic::vertex_id_t id, bool for_forward_direct)
    {
        graph_basic::vertex_t& v = get_vertex_by_id(id);
        if (direction == undirectional)
            return graph_basic::math::my_astar_heuristic( v, finish_lat_, finish_lon_ );
        else
        {
            if (for_forward_direct)
                return potential_fwd(v);
            else
                return potential_revs(v);
        }
    }
    template<astar_mode_e mode, astar_direction_e direction>
    bool astar_search_t<mode, direction>::is_all_ended()
    {
        if (direction == undirectional)
        {
            return is_all1_ended;
        }
        else if (direction == bidirectional)
        {
            return is_all1_ended && is_all2_ended;
        }
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::init_state()
    {        
        is_all1_ended = false;     
        put_in_opened_list(start_vertex_id_);
        put_distance(start_vertex_id_, 0);
        current_v_id = start_vertex_id_;
        if (direction == bidirectional)
        {
            is_all2_ended = false;
            put_in_opened_list2(finish_vertex_id_);
            put_distance2(finish_vertex_id_, 0);
            
            current_v2_id = finish_vertex_id_;
            min_2direct_dist = graph_basic::MAX_FLOAT;
            
            graph_basic::vertex_t& start_v = get_vertex_by_id(start_vertex_id_);
            graph_basic::vertex_t& finish_v = get_vertex_by_id(finish_vertex_id_);

            sf_dist_ = graph_basic::math::my_astar_heuristic(start_v, finish_v);
            beta_ = sf_dist_/10;            
        }
        //debug
        if (mode == debug)
        {
            num_iterates_ = 0;
            num_iterates2_ = 0;
            visited_vertices_ = 0;
            visited_vertices2_ = 0;
            visited_edges_ = 0;
            visited_edges2_ = 0;

            graph_basic::vertex_id_t random_id = (*vertices(view_)).get_id();
            deb_vertex_current1_.id_ = random_id;
            deb_vertex_current2_.id_ = random_id;
            deb_vertex_prev1_.id_ = random_id;
            deb_vertex_prev2_.id_ = random_id;
            //calls statistics        
            opened_list1_put_calls_ = 0;
            opened_list1_isin_calls_ = 0;
            opened_list1_extract_min_calls_ = 0;
            opened_list1_chpriority_calls_ = 0;
            opened_list1_max_elems_ = 0;
            closed_list1_put_calls_ = 0;
            closed_list1_isin_calls_ = 0;
            put_predecessor_calls_ = 0;
            get_distance_calls_ = 0;
        }
    }
    template<astar_mode_e mode, astar_direction_e direction>
    graph_basic::lat_lon_t astar_search_t<mode, direction>::potential_fwd(graph_basic::vertex_t& v)
    {        
        graph_basic::lat_lon_t val1 = graph_basic::math::my_astar_heuristic(v, finish_lat_, finish_lon_ );
        graph_basic::lat_lon_t val2 = sf_dist_ - graph_basic::math::my_astar_heuristic(v, start_lat_, start_lon_) + beta_;
        return val1 > val2? val1 : val2 ;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    graph_basic::lat_lon_t astar_search_t<mode, direction>::potential_revs(graph_basic::vertex_t& v)
    {        
        graph_basic::lat_lon_t val1 = graph_basic::math::my_astar_heuristic(v, finish_lat_, finish_lon_ );
        graph_basic::lat_lon_t val2 = sf_dist_ - graph_basic::math::my_astar_heuristic(v, start_lat_, start_lon_) + beta_;
        return val1 > val2? -val1 : -val2 ;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    graph_basic::vertex_id_t astar_search_t<mode, direction>::extract_min()
    {
        if (mode == debug)
            {++opened_list1_extract_min_calls_;}

        distance_map_t::iterator it = opened_list1_.begin();
        distance_map_t::iterator min_it = it;
        distance_map_t::iterator end_it = opened_list1_.end();
        graph_basic::lat_lon_t min = graph_basic::MAX_FLOAT;
        graph_basic::vertex_id_t min_id(0,0);

        for(;it!=end_it;++it)
        {
            graph_basic::vertex_t& it_vertex = view_->get_vertex_by_id((*it).first);
            graph_basic::lat_lon_t cost = (*it).second;
            //if (direction == undirectional)
            //    {cost += graph_basic::math::my_astar_heuristic(it_vertex, finish_lat_, finish_lon_ ); }
            if (cost < min)
            {
                min = cost;
                min_id = (*it).first;
                min_it = it;
            }
        }
        opened_list1_.erase(min_it);
        return min_id;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    graph_basic::vertex_id_t astar_search_t<mode, direction>::extract_min2()
    {
        distance_map_t::iterator it = opened_list2_.begin();
        distance_map_t::iterator min_it = it;
        distance_map_t::iterator end_it = opened_list2_.end();
        graph_basic::lat_lon_t min = graph_basic::MAX_FLOAT;
        for(;it!=end_it;++it)
        {
            graph_basic::vertex_t& it_vertex = view_->get_vertex_by_id((*it).first);
            graph_basic::lat_lon_t cost = (*it).second;
/*            if (direction == undirectional)
                {cost += graph_basic::math::my_astar_heuristic(it_vertex, start_lat_, start_lon_); }     */           
            if (cost < min)
            {
                min = cost;
                min_it = it;
            }
        }
        graph_basic::vertex_id_t min_id = (*min_it).first;
        opened_list2_.erase(min_it);
        //return v;
        //vertex_data2_[min_id].color_ = astar_vertex_data_t::OPENED_LIST;
        return min_id;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::put_in_opened_list(graph_basic::vertex_id_t id)
    {
        if (mode == debug)
        {
            ++opened_list1_put_calls_;
            if (opened_list1_.size() > opened_list1_max_elems_)
                opened_list1_max_elems_ = opened_list1_.size();
        }
        opened_list1_[id] = graph_basic::MAX_FLOAT;
        notify_priority_changed(id);
        vertex_data1_[id].color_ = astar_vertex_data_t::OPENED_LIST;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::put_in_opened_list2(graph_basic::vertex_id_t id)
    {
        opened_list2_[id] = graph_basic::MAX_FLOAT;
        notify_priority_changed2(id);
        vertex_data2_[id].color_ = astar_vertex_data_t::OPENED_LIST;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::put_in_closed_list(graph_basic::vertex_id_t id)
    {
        if (mode == debug)
            {++closed_list1_put_calls_;}
        vertex_data1_[id].color_ = astar_vertex_data_t::CLOSED_LIST;
        opened_list1_.erase(id);
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::put_in_closed_list2(graph_basic::vertex_id_t id)
    {
        vertex_data2_[id].color_ = astar_vertex_data_t::CLOSED_LIST;
        opened_list2_.erase(id);
    }
    template<astar_mode_e mode, astar_direction_e direction>
    bool astar_search_t<mode, direction>::is_in_opened_list(graph_basic::vertex_id_t id)
    {
        if (mode == debug)
            {++opened_list1_isin_calls_;}
        distance_map_t::iterator it1 = opened_list1_.find(id);
        if (it1 != opened_list1_.end())
        {
            return true;
        }
        else return false;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    bool astar_search_t<mode, direction>::is_in_opened_list2(graph_basic::vertex_id_t id)
    {
        distance_map_t::iterator it1 = opened_list2_.find(id);
        if (it1 != opened_list2_.end())
        {
            return true;
        }
        else return false;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    bool astar_search_t<mode, direction>::is_in_closed_list(graph_basic::vertex_id_t id)
    {
        if (mode == debug)
            {++closed_list1_isin_calls_;}
        bool b1 = vertex_data1_.has_key(id);
        if (!b1) return false;
        bool b2 = vertex_data1_.get_value(id).color_ == astar_vertex_data_t::CLOSED_LIST;
        return b1 && b2;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    bool astar_search_t<mode, direction>::is_in_closed_list2(graph_basic::vertex_id_t id)
    {
        bool b1 = vertex_data2_.has_key(id);
        if (!b1) return false;
        bool b2 = vertex_data2_.get_value(id).color_ == astar_vertex_data_t::CLOSED_LIST;
        return b1 && b2;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    bool astar_search_t<mode, direction>::is_opened_list_empty()
    {
        return opened_list1_.empty();
    }
    template<astar_mode_e mode, astar_direction_e direction>
    bool astar_search_t<mode, direction>::is_opened_list2_empty()
    {
        return opened_list2_.empty();
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::notify_priority_changed(graph_basic::vertex_id_t id)
    {
        if (mode == debug)
            {++opened_list1_chpriority_calls_;}
        //do nothing
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::notify_priority_changed2(graph_basic::vertex_id_t id)
    {
        //do nothing
    }
    template<astar_mode_e mode, astar_direction_e direction>
    graph_basic::lat_lon_t astar_search_t<mode, direction>::get_distance(graph_basic::vertex_id_t id)
    {
        ++get_distance_calls_;
        return vertex_data1_.get_value(id).distance_;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    graph_basic::lat_lon_t astar_search_t<mode, direction>::get_distance2(graph_basic::vertex_id_t id)
    {
        return vertex_data2_.get_value(id).distance_;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::put_distance(graph_basic::vertex_id_t id, graph_basic::lat_lon_t dist)
    {
        opened_list1_[id] = dist;
        vertex_data1_[id].distance_ = dist;
        notify_priority_changed(id);
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::put_distance2(graph_basic::vertex_id_t id, graph_basic::lat_lon_t dist)
    {
        opened_list2_[id] = dist;
        vertex_data2_[id].distance_ = dist;
        notify_priority_changed2(id);
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::put_predecessor(graph_basic::vertex_id_t id_v, graph_basic::vertex_id_t id_pred)
    {
        ++put_predecessor_calls_;
        vertex_data1_[id_v].predecessor_ = id_pred;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::put_predecessor2(graph_basic::vertex_id_t id_v, graph_basic::vertex_id_t id_pred)
    {
        vertex_data2_[id_v].predecessor_ = id_pred;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::collect_stats1()
    {
         //collecting statistics
            ++num_iterates_;
            graph_basic::vertex_t& current_v = view_->get_vertex_by_id(current_v_id); 
            graph_basic::vertex_t& prev_v = view_->get_vertex_by_id(deb_vertex_prev1_.id_); 

            deb_vertex_current1_.distance_ = get_distance(current_v_id);            
            deb_vertex_current1_.heuristic_ = graph_basic::math::my_astar_heuristic(current_v, finish_lat_, finish_lon_ );
            deb_vertex_current1_.lat_ = current_v.get_point().lat_;
            deb_vertex_current1_.lon_ = current_v.get_point().lon_;
            deb_vertex_current1_.id_ = current_v_id;

            deb_vertex_prev1_.distance_ = get_distance(deb_vertex_prev1_.id_);
            deb_vertex_prev1_.heuristic_ = graph_basic::math::my_astar_heuristic(prev_v, finish_lat_, finish_lon_ );
            deb_vertex_prev1_.lat_ = prev_v.get_point().lat_;
            deb_vertex_prev1_.lon_ = prev_v.get_point().lon_;
            //adding adjacent vertices, which are not processed yet
            deb_vertex_prev1_adj.erase(deb_vertex_prev1_adj.begin(), deb_vertex_prev1_adj.end());
            virtual_view_adjacent_vertices_iterator_t prev_adj_v_it_pair = adjacent_vertices(deb_vertex_prev1_.id_, view_);
            for(; !prev_adj_v_it_pair.is_empty(); ++prev_adj_v_it_pair)
            {
                if (!is_in_closed_list( (*prev_adj_v_it_pair).get_id() ))
                {
                    vertex_debug_t v_d;
                    v_d.id_ = (*prev_adj_v_it_pair).get_id();
                    v_d.distance_ = get_distance((*prev_adj_v_it_pair).get_id());
                    v_d.heuristic_ = graph_basic::math::my_astar_heuristic((*prev_adj_v_it_pair), finish_lat_, finish_lon_ );
                    v_d.lat_ = (*prev_adj_v_it_pair).get_point().lat_;
                    v_d.lon_ = (*prev_adj_v_it_pair).get_point().lon_;
                    deb_vertex_prev1_adj.push_back(v_d);
                }
            }
            //end adding adj vertices
            deb_vertex_prev1_.id_ = current_v_id;

            //end collecting statistics
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::process_current_adjacent1()
    {
         virtual_view_adjacent_vertices_iterator_t it = adjacent_vertices(current_v_id, view_);
         for(;!it.is_empty(); ++it)
         {
             ++visited_edges_;
             if (!is_in_closed_list( (*it).get_id() ))
             {                    
                 graph_basic::vertex_t& current_v = view_->get_vertex_by_id(current_v_id);                   
                 if (!is_in_opened_list( (*it).get_id() ))
                 {
                     //put_in_opened_list( (*it).id_ );
                     graph_basic::lat_lon_t new_dist = get_distance(current_v_id) +                    
                         graph_basic::math::distance( (*it).get_point().lat_, (*it).get_point().lon_, current_v.get_point().lat_, current_v.get_point().lon_ );
                     if (direction == undirectional) 
                         new_dist+= -graph_basic::math::my_astar_heuristic(current_v, finish_lat_, finish_lon_ ) + graph_basic::math::my_astar_heuristic((*it), finish_lat_, finish_lon_ );
                     else
                         { new_dist+= -potential_fwd(current_v) + potential_fwd((*it));} //changing length of the edge by potential function
                     put_distance((*it).get_id(), new_dist);
                     put_predecessor( (*it).get_id(), current_v_id);

                     if (mode == debug)
                     {
                     ++visited_vertices_;
                     //drawing
                     output_list_.push_back(line_t(
                         graph_basic::point_t(current_v.get_point().lat_,current_v.get_point().lon_),
                         graph_basic::point_t((*it).get_point().lat_,(*it).get_point().lon_))
                         ); 
                     }
                 }
                 else
                 {
                     graph_basic::lat_lon_t new_dist = get_distance(current_v_id) + 
                         graph_basic::math::distance( (*it).get_point().lat_, (*it).get_point().lon_, current_v.get_point().lat_, current_v.get_point().lon_ );
                     if (direction == undirectional) 
                         new_dist+= -graph_basic::math::my_astar_heuristic(current_v, finish_lat_, finish_lon_ ) + graph_basic::math::my_astar_heuristic((*it), finish_lat_, finish_lon_ );
                     else
                         { new_dist+= -potential_fwd(current_v) + potential_fwd((*it));} //changing length of the edge by potential function

                     if (get_distance((*it).get_id()) > new_dist)
                     {
                         put_distance((*it).get_id(), new_dist);
                         put_predecessor( (*it).get_id(), current_v_id);
                     }
                 }
                 
                 //checking exit conditions for symmentric approach
                 if (direction == bidirectional)
                 {
                     /*if (is_in_opened_list2( (*it).id_ ) || is_in_closed_list2((*it).id_))
                     {
                         graph_basic::vertex_t& current_v = view_->get_vertex_by_id(current_v_id);
                         graph_basic::vertex_t& next_v = view_->get_vertex_by_id((*it).id_);
                         
                         graph_basic::lat_lon_t path_len = 0;
                         graph_basic::lat_lon_t temp = get_distance(current_v_id);
                         path_len+= temp;
                         temp = get_distance2(next_v.id_);
                         path_len+= temp;
                         temp = Distance( current_v.latitude_, current_v.longtitude_, next_v.latitude_, next_v.longtitude_);
                         path_len+= temp;

                         if (path_len < min_2direct_dist)
                         {
                             min_2direct_dist = path_len;
                         }
                         else if( get_distance(next_v.id_) + graph_basic::math::my_astar_heuristic(next_v , finish_lat_, finish_lon_)
                         >= min_2direct_dist)
                         {
                             sh_path_vert_dim1 = current_v_id;
                             sh_path_vert_dim2 = next_v.id_;
                             is_all1_ended = 1;
                             is_all2_ended = 1;
                             break;
                         }
                     }*/
                         
                 }
             }
         }
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::run1(bool iterate, unsigned long long iteration_number)
    {     
        if (mode == debug)
        {
            bool old_iterate = iterate;
            iterate = true;
            while( 
                (current_v_id != finish_vertex_id_) && 
                (!is_opened_list_empty()) && 
                iterate && 
                (num_iterates_ < iteration_number || iteration_number == 0))
            {
                if (!old_iterate) iterate = false;
                current_v_id = extract_min();
                put_in_closed_list(current_v_id);
                collect_stats1(); 
                process_current_adjacent1();
            }
        } 
        else if (mode == release)
        {
            while( 
                 (current_v_id != finish_vertex_id_) && 
                 (!is_opened_list_empty()))
            {
                current_v_id = extract_min();
                put_in_closed_list(current_v_id);
                process_current_adjacent1();
            }
        }
        if (current_v_id == finish_vertex_id_) 
            is_all1_ended = true;
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::collect_stats2()
    {
        //collecting statistics
        ++num_iterates2_;
        graph_basic::vertex_t& current_v2 = view_->get_vertex_by_id(current_v2_id); 
        graph_basic::vertex_t& prev_v2 = view_->get_vertex_by_id(deb_vertex_prev2_.id_); 

        deb_vertex_current2_.distance_ = get_distance2(current_v2_id);            
        deb_vertex_current2_.heuristic_ = graph_basic::math::my_astar_heuristic(current_v2, start_lat_, start_lon_ );
        deb_vertex_current2_.lat_ = current_v2.get_point().lat_;
        deb_vertex_current2_.lon_ = current_v2.get_point().lon_;
        deb_vertex_current2_.id_ = current_v2_id;

        deb_vertex_prev2_.distance_ = get_distance2(deb_vertex_prev2_.id_);
        deb_vertex_prev2_.heuristic_ = graph_basic::math::my_astar_heuristic(prev_v2, start_lat_, start_lon_ );
        deb_vertex_prev2_.lat_ = prev_v2.get_point().lat_;
        deb_vertex_prev2_.lon_ = prev_v2.get_point().lon_;
        //adding adjacent vertices, which are not processed yet
        deb_vertex_prev2_adj.erase(deb_vertex_prev2_adj.begin(), deb_vertex_prev2_adj.end());
        virtual_view_adjacent_vertices_iterator_t prev_adj_v_it_pair2 = adjacent_vertices(deb_vertex_prev2_.id_, view_);
        for(;!prev_adj_v_it_pair2.is_empty(); ++prev_adj_v_it_pair2)
        {
            if (!is_in_closed_list2( (*prev_adj_v_it_pair2).get_id() ))
            {
                vertex_debug_t v_d;
                v_d.id_ = (*prev_adj_v_it_pair2).get_id();
                v_d.distance_ = get_distance2((*prev_adj_v_it_pair2).get_id());
                v_d.heuristic_ = graph_basic::math::my_astar_heuristic((*prev_adj_v_it_pair2), start_lat_, start_lon_ );
                v_d.lat_ = (*prev_adj_v_it_pair2).get_point().lat_;
                v_d.lon_ = (*prev_adj_v_it_pair2).get_point().lon_;
                deb_vertex_prev2_adj.push_back(v_d);
            }
        }
        //end adding adj vertices
        deb_vertex_prev2_.id_ = current_v2_id;

        //end collecting statistics

    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::process_current_adjacent2()
    {                                                             
        virtual_view_adjacent_vertices_iterator_t it2 = adjacent_vertices(current_v2_id, view_);

        for(; !it2.is_empty();++it2)
        {
            ++visited_edges2_;
            if (!is_in_closed_list2( (*it2).get_id() ))
            {                        
                graph_basic::vertex_t& current_v2 = view_->get_vertex_by_id(current_v2_id);                   
                if (!is_in_opened_list2( (*it2).get_id() ))
                {
                    //put_in_opened_list2( (*it2).id_ );
                    graph_basic::lat_lon_t new_dist = get_distance2(current_v2_id) + 
                        graph_basic::math::distance( (*it2).get_point().lat_, (*it2).get_point().lon_, current_v2.get_point().lat_, current_v2.get_point().lon_ );
                    if (direction == undirectional) 
                            new_dist+= -graph_basic::math::my_astar_heuristic(current_v2, start_lat_, start_lon_ ) + graph_basic::math::my_astar_heuristic((*it2), start_lat_, start_lon_ );
                    else
                        { new_dist+= -potential_revs(current_v2) + potential_revs((*it2));} //changing length of the edge by potential function
                    put_distance2((*it2).get_id(), new_dist);
                    put_predecessor2( (*it2).get_id(), current_v2_id);
                    if (mode == debug)
                    {
                       ++visited_vertices2_;
                       //drawing
                       output_list2_.push_back(line_t(
                           graph_basic::point_t(current_v2.get_point().lat_,current_v2.get_point().lon_),
                           graph_basic::point_t((*it2).get_point().lat_,(*it2).get_point().lon_))
                           );
                   }
                }
                else
                {
                    graph_basic::lat_lon_t new_dist = get_distance2(current_v2_id) + 
                        graph_basic::math::distance( (*it2).get_point().lat_, (*it2).get_point().lon_, current_v2.get_point().lat_, current_v2.get_point().lon_ );
                    if (direction == undirectional) 
                        new_dist+= -graph_basic::math::my_astar_heuristic(current_v2, start_lat_, start_lon_ ) + graph_basic::math::my_astar_heuristic((*it2), start_lat_, start_lon_ );
                    else
                        { new_dist+= -potential_revs(current_v2) + potential_revs((*it2));} //changing length of the edge by potential function
                    if (get_distance2((*it2).get_id()) > new_dist)
                    {
                        put_distance2((*it2).get_id(), new_dist);
                        put_predecessor2( (*it2).get_id(), current_v2_id);
                    }
                }        
                //check exit condition for symmetric approach
                //if (direction == bidirectional)
                //{
                //    if (is_in_opened_list( (*it2).id_ ) || is_in_closed_list((*it2).id_))
                //    {
                //        graph_basic::vertex_t& current_v2 = view_->get_vertex_by_id(current_v2_id);
                //        graph_basic::vertex_t& next_v = view_->get_vertex_by_id((*it2).id_);
                //        
                //        graph_basic::lat_lon_t path_len = 0;
                //        graph_basic::lat_lon_t temp = get_distance2(current_v2_id);
                //        path_len+= temp;
                //        temp = get_distance(next_v.id_);
                //        path_len+= temp;
                //        temp = Distance( current_v2.latitude_, current_v2.longtitude_, next_v.latitude_, next_v.longtitude_);
                //        path_len+= temp;

                //        if (path_len < min_2direct_dist)
                //        {
                //            min_2direct_dist = path_len;
                //        }
                //        else if( get_distance2(next_v.id_) + graph_basic::math::my_astar_heuristic(next_v , start_lat_, start_lon_)
                //        >= min_2direct_dist)
                //        {
                //            sh_path_vert_dim1 = next_v.id_;
                //            sh_path_vert_dim2 = current_v2_id;
                //            is_all1_ended = 1;
                //            is_all2_ended = 1;
                //            break;
                //        }
                //    }
                //}
            }

        }
    }
    template<astar_mode_e mode, astar_direction_e direction>
    void astar_search_t<mode, direction>::run2(bool iterate, unsigned long long iteration_number)
    {          
        do
        {            
            if (!is_all1_ended)
            {
                current_v_id = extract_min();
                put_in_closed_list(current_v_id);
                if (mode == debug)
                    {collect_stats1();}
                if (is_in_closed_list2(current_v_id))
                {
                    sh_path_vert_dim1 = current_v_id;
                    sh_path_vert_dim2 = current_v_id;
                    is_all1_ended = 1;
                    is_all2_ended = 1;
                    break;
                }
                process_current_adjacent1();
            }
            if (!is_all2_ended)
            {                
                current_v2_id = extract_min2();                
                put_in_closed_list2(current_v2_id);
                if (mode == debug)
                    {collect_stats2();}
                if (is_in_closed_list(current_v2_id))
                {
                    sh_path_vert_dim1 = current_v2_id;
                    sh_path_vert_dim2 = current_v2_id;
                    is_all1_ended = 1;
                    is_all2_ended = 1;
                    break;
                }
                process_current_adjacent2();                
            }
        }
        while( 
            (!is_all1_ended || !is_all2_ended) && 
            (!is_opened_list_empty()) && 
            (!is_opened_list2_empty()) &&
            ( mode == release ||
            (iterate && ((num_iterates_ < iteration_number && num_iterates2_ < iteration_number) || iteration_number == 0))
            )
            );
    }
    /*
   params:
   @list - std:::list<lat_lon_t>, contained result
   */
   template<astar_mode_e mode, astar_direction_e direction>
   inline void fill_shpath(astar_search_t<mode, direction>& as, std::list<graph_basic::lat_lon_t>& list)
   {
       if (direction == undirectional)
       {
           graph_basic::vertex_id_t finish_id = as.get_finish_vertex_id();
           graph_basic::vertex_id_t start_id = as.get_start_vertex_id();
           graph_basic::vertex_id_t temp_id = as.get_finish_vertex_id();
       
           vertex_data_container_t<astar_vertex_data_t>& vert_data = as.get_vertex_data();
           while (temp_id != start_id)
           {
               graph_basic::vertex_t& temp_v = as.get_vertex_by_id(temp_id);
               list.push_back(temp_v.get_point().lat_);
               list.push_back(temp_v.get_point().lon_);       
               temp_id = vert_data.get_value(temp_id).predecessor_;
           }
       }
       else if (direction == bidirectional)
       {
           vertex_data_container_t<astar_vertex_data_t> dim1_v_data = as.get_vertex_data();
           vertex_data_container_t<astar_vertex_data_t> dim2_v_data = as.get_vertex_data2();
           //std::list<lat_lon_t> std_list;

           graph_basic::vertex_id_t finish_id = as.get_finish_vertex_id();
           graph_basic::vertex_id_t start_id = as.get_start_vertex_id();
           graph_basic::vertex_id_t temp_id = as.get_fwd_search_cur_vertex_id();

           while (temp_id != start_id)
           {
               graph_basic::vertex_t& temp_v = as.get_vertex_by_id(temp_id);
               list.push_back(temp_v.get_point().lat_);
               list.push_back(temp_v.get_point().lon_);
               temp_id = dim1_v_data.get_value(temp_id).predecessor_;
           }
           graph_basic::vertex_t* temp_v = &as.get_vertex_by_id(temp_id);
           list.push_back(temp_v->get_point().lat_);
           list.push_back(temp_v->get_point().lon_); 

           temp_id = as.get_bkwd_search_cur_vertex_id();
           while (temp_id != finish_id)
           {
               graph_basic::vertex_t& temp_v = as.get_vertex_by_id(temp_id);
               list.insert(list.begin(), temp_v.get_point().lon_);
               list.insert(list.begin(), temp_v.get_point().lat_);            
               temp_id = dim2_v_data.get_value(temp_id).predecessor_;
           }
           temp_v = &as.get_vertex_by_id(temp_id);
           list.insert(list.begin(), temp_v->get_point().lon_);
           list.insert(list.begin(), temp_v->get_point().lat_);  
       }      
   }
   template<astar_mode_e mode, astar_direction_e direction>
   graph_basic::vertex_id_t astar_search_t<mode, direction>::get_start_vertex_id() const
   {
      return start_vertex_id_;
   }
   template<astar_mode_e mode, astar_direction_e direction>
   graph_basic::vertex_id_t astar_search_t<mode, direction>::get_finish_vertex_id() const
   {
      return finish_vertex_id_;
   }
   template<astar_mode_e mode, astar_direction_e direction>
   graph_basic::vertex_id_t astar_search_t<mode, direction>::get_fwd_search_cur_vertex_id() const
   {
      return sh_path_vert_dim1;
   }
   template<astar_mode_e mode, astar_direction_e direction>
   graph_basic::vertex_id_t astar_search_t<mode, direction>::get_bkwd_search_cur_vertex_id() const
   {
      return sh_path_vert_dim2;
   }
}
}